﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Drifting.Graphics.EffectObjects;
using Drifting.MotionPlanning.Primitives;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Drifting.Foundation;

namespace Drifting.Graphics.Visualizers
{
    public class Skin_OneLink : Skin_G
    {
        public Skin_OneLink(List<State> train, GraphicsDevice device)
            : base (train, device)
        {
        }

        override public bool TranslateStateForVisualization(State x, ref float[] q_tmp, ref Color colorOfCost, double costBound)
        {
            if (x == null) return false;
            double[] q = x.Q;
            if (q == null) return false;

            double cost = x.Cost;
            double normalizedCost = 1 - cost / costBound;
            double complementaryNormalizedColor = 1 - normalizedCost;

            Color lowColor = Color.Gold;
            Color highColor = Color.Blue;
            colorOfCost.R = (byte)(normalizedCost * lowColor.R + complementaryNormalizedColor * highColor.R);
            colorOfCost.G = (byte)(normalizedCost * lowColor.G + complementaryNormalizedColor * highColor.G);
            colorOfCost.B = (byte)(normalizedCost * lowColor.B + complementaryNormalizedColor * highColor.B);

            q_tmp[0] = (float)q[0];
            q_tmp[1] = (float)q[1] / 10 * 3.1415926f;
            q_tmp[2] = (float)(complementaryNormalizedColor * 5);

            return true;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {

            if (train == null) return;
            lock (train)
            {
                int step_size = 1;
                int idx_start = rand.Next(train.Count);

                int step_size_low = (int)(train.Count / 5000f);
                if (step_size_low < 1)
                    step_size_low = 1;

                if (preview == PreviewResolution.Low)
                    step_size = step_size_low;
                else if (preview == PreviewResolution.Median)
                {
                    int step_size_probe = (int)Math.Floor(Math.Sqrt(train.Count));
                    if (step_size_probe > step_size_low)
                        step_size = step_size_low;
                    else
                        step_size = step_size_probe;
                }
                else
                    step_size = 1;

                Color colorOfCost = Color.Black;
                double costBound = 7;

                for (int i = idx_start; i < train.Count; i += step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states

                    if (q1_tmp[0] > 0)
                        q1_tmp[0] -= MathAssistant.PI_double_float;
                    else
                        q1_tmp[0] += MathAssistant.PI_double_float;

                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the image state
                }
                for (int i = idx_start - 1; i >= 0; i -= step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states

                    if (q1_tmp[0] > 0)
                        q1_tmp[0] -= MathAssistant.PI_double_float;
                    else
                        q1_tmp[0] += MathAssistant.PI_double_float;

                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the image state
                }
            }

        }
    }
}
